/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import file.Configurable;
import file.Configuration;

import file.DataRecorder;

/**
 *
 * @author Mentors  Carl Burger - Team 708
 */
public class Gripper implements Configurable{

    private static DoubleSolenoid gripperSolenoid;
    private static DataRecorder   dataRecorder;
    private static MessageCenter  messageCenter;

    private static boolean        previousGripperPos;  // for message center display

    private static Gripper        m_instance = null;
    private static DigitalInput   irSensor   = null;

    private double gripperDelay = 0; //time it takes to open the gripper

    //boolean toggle = false;


    /**
     *  Get an instance of the Gripper
     */
//    public static Gripper getInstance(int actuatorChannel, int sensorChannel) {
    public static Gripper getInstance() {

        if (m_instance == null) {

            m_instance = new Gripper();

            Configuration.getInstance().register("Gripper", m_instance);
            
        }

        return m_instance;
    }

    private Gripper() {

        irSensor = new DigitalInput(LogomotionRobot.kGripperIRSensor);
        previousGripperPos = irSensor.get();

        gripperSolenoid = new DoubleSolenoid(LogomotionRobot.kGripperForward,
                                             LogomotionRobot.kGripperReverse);

        // if the next statement is removed, then the gripper cannot be moved at all
        // it's not obvious why this statement is needed
        // this statement closes the gripper
        gripperSolenoid.set(DoubleSolenoid.Value.kReverse);

        dataRecorder = DataRecorder.getInstance();

        messageCenter = MessageCenter.getInstance();

    }

    public void checkForTube() {

        // if the sensor detects a tube, then close the gripper
        grasp();
    }

    public void grasp() {
        gripperSolenoid.set(DoubleSolenoid.Value.kReverse);
    }

    public void release() {
        gripperSolenoid.set(DoubleSolenoid.Value.kForward);
    }

    public boolean hasShape() {
        return false;
//        return irSensor.get();
    }

    public void toggle() {

        if (gripperSolenoid.get().value == DoubleSolenoid.Value.kForward.value) {
            gripperSolenoid.set(DoubleSolenoid.Value.kReverse);
        }
        else {
            gripperSolenoid.set(DoubleSolenoid.Value.kForward);
        }
    }

    /**
     * displaySensorStatus is called from disabledPeriodic and teleopPeriodic
     * to display the sensor value
     */
    public void displaySensorStatus() {

        boolean gripperpos = irSensor.get();

        if(gripperpos != previousGripperPos){
            previousGripperPos = gripperpos;

            messageCenter.println(MessageCenter.kSensorsScreen, MessageCenter.kLine3,
                                  "Gripper: " + ((gripperpos)?"shape":"no shape"));
        }

        dataRecorder.record("gripper",gripperpos);
    }

    public double getDelay(){
        return gripperDelay;
    }

    public void update(String name, String[] values) {
        if(name.equals("Open.Delay")){
            gripperDelay = Double.parseDouble(values[0]);
        }
    }
}